/**
 * \file Quaternion.cpp
 * \brief Quaternion declaration class
 * \author Edouard ROGE
 * \date 20 decembre 2010
 */

#include "Matrix.h"
#include "Point3D.h"
#include "Vector3D.h"
#include "Quaternion.h"


namespace Math
{
	namespace M3D
	{
		Quaternion::Quaternion()
			: x(0), y(0), z(0), w(0)
		{
		}

		Quaternion::Quaternion(float _x, float _y, float _z, float _w)
			: x(_x), y(_y), z(_z), w(_w)
		{
		}

		Quaternion::Quaternion(const Matrix<float> & _mMatrix)
		{
			CreateFromMatrix(_mMatrix);
		}

		Quaternion::Quaternion(const Vector3D<float> & _fAxis, float _fAngle)
		{
			CreateFromAxis(_fAxis, _fAngle);
		}

		Quaternion::~Quaternion()
		{
		}

		void Quaternion::Set(float _x, float _y, float _z, float _w)
		{
			x = _x;
			y = _y;
			z = _z;
			w = _w;
		}

		void Quaternion::CreateFromMatrix(const Matrix<float> & _mMatrix)
		{
			// TODO("_mMatrix must be ortho : Mt * M = I")
			float fTrace = _mMatrix(0, 0) + _mMatrix(1, 1) + _mMatrix(2, 2);

			if (fTrace > 0.0f)
			{
				// TODO( "Check matrix RH or LH... doc on it" )
				float s = 1.0f / (2.0f * sqrt(fTrace));
				x = (_mMatrix(2, 1) - _mMatrix(1, 2)) * s;
				y = (_mMatrix(0, 2) - _mMatrix(2, 0)) * s;
				z = (_mMatrix(1, 0) - _mMatrix(0, 1)) * s;
				w = 1.0f / (4.0f * s);
			}
			else
			{
				if (_mMatrix(0, 0) > _mMatrix(1, 1) && _mMatrix(0, 0) > _mMatrix(2, 2))
				{
					float s = 2.0f * sqrt(1.0f + _mMatrix(0, 0) - _mMatrix(1, 1) - _mMatrix(2, 2));
					x = 1.0f / (2.0f * s);
					y = (_mMatrix(0, 1) - _mMatrix(1, 0)) / s;
					z = (_mMatrix(0, 2) - _mMatrix(2, 0)) / s;
					w = (_mMatrix(1, 2) - _mMatrix(2, 1)) / s;
				}
				else if (_mMatrix(1, 1) > _mMatrix(2, 2))
				{
					float s = 2.0f * sqrt(1.0f - _mMatrix(0, 0) + _mMatrix(1, 1) - _mMatrix(2, 2));
					x = (_mMatrix(0, 1) - _mMatrix(1, 0)) / s;
					y = 1.0f / (2.0f * s);
					z = (_mMatrix(1, 2) - _mMatrix(2, 1)) / s;
					w = (_mMatrix(0, 2) - _mMatrix(2, 0)) / s;
				}
				else
				{
					float s = 2.0f * sqrt(1.0f - _mMatrix(0, 0) - _mMatrix(1, 1) + _mMatrix(2, 2));
					x = (_mMatrix(0, 2) - _mMatrix(2, 0)) / s;
					y = (_mMatrix(1, 2) - _mMatrix(2, 1)) / s;
					z = 1.0f / (2.0f * s);
					w = (_mMatrix(0, 1) - _mMatrix(1, 0)) / s;
				}
			}
		}

		void Quaternion::CreateFromAxis(const Vector3D<float> & _fAxis, float _fAngle)
		{
			float fSin = sin(_fAngle * 0.5f);
			float fCos = cos(_fAngle * 0.5f);

			x = _fAxis.x * fSin;
			y = _fAxis.y * fSin;
			z = _fAxis.z * fSin;
			w = fCos;
		}

		Quaternion Quaternion::Conjugate() const
		{
			return Quaternion(-x, -y, -z, w);
		}

		float Quaternion::GetLength() const
		{
			return sqrt(x*x + y*y + z*z + w*w);
		}

		Quaternion Quaternion::Normalize() const
		{
			float fLength = GetLength();
			//LOG_ASSERT(fLength != 0, "Normalize null quaternion");
			return (*this) / fLength;
		}

		void Quaternion::GetMatrix(Matrix<float> & _Matrix) const
		{
			_Matrix[0] = 1.0f - 2.0f * (y*y + z*z);
			_Matrix[1] = 2.0f * (x*y - z*w);
			_Matrix[2] = 2.0f * (x*z + y*w);

			_Matrix[4] = 2.0f * (x*y + z*w);
			_Matrix[5] = 1.0f - 2.0f * (x*x + z*z);
			_Matrix[6] = 2.0f * (y*z - x*w);

			_Matrix[8] = 2.0f * (x*z - y*w);
			_Matrix[9] = 2.0f * (y*z + x*w);
			_Matrix[10] = 1.0f - 2.0f * (x*x + y*y);
		}

		void Quaternion::GetAxis(Vector3D<float> & _fAxis, float & _fAngle) const
		{
			Quaternion q = Normalize();
			_fAngle = acos(q.w) * 2.0f;

			_fAxis.x = q.x;
			_fAxis.y = q.y;
			_fAxis.z = q.z;

			_fAxis = _fAxis.Normalize();
		}

		Quaternion::operator float * ()
		{
			return values;
		}

		Quaternion::operator const float * () const
		{
			return values;
		}

		Quaternion Quaternion::operator - () const
		{
			return Conjugate();
		}


		Quaternion & Quaternion::operator *= (float _f)
		{
			x *= _f;
			y *= _f;
			z *= _f;
			w *= _f;
			return (*this);
		}

		Quaternion & Quaternion::operator /= (float _f)
		{
			//LOG_ASSERT((_f != 0.0f), "Divide by 0");
			x /= _f;
			y /= _f;
			z /= _f;
			w /= _f;
			return (*this);
		}

		Quaternion & Quaternion::operator += (const Quaternion & _q)
		{
			x += _q.x;
			y += _q.y;
			z += _q.z;
			w += _q.w;
			return (*this);
		}

		Quaternion & Quaternion::operator -= (const Quaternion & _q)
		{
			x -= _q.x;
			y -= _q.y;
			z -= _q.z;
			w -= _q.w;
			return (*this);
		}

		Quaternion & Quaternion::operator *= (const Quaternion & _q)
		{
			Set(x * _q.w + w * _q.x + z * _q.y - y * _q.z,
				y * _q.w + w * _q.y + x * _q.z - z * _q.x,
				z * _q.w + w * _q.z + y * _q.x - x * _q.y,
				w * _q.w - x * _q.x - y * _q.y - z * _q.z);

			return (*this);
		}

		Quaternion & Quaternion::operator /= (const Quaternion & _q)
		{
			Set(x * _q.w - w * _q.x - z * _q.y + y * _q.z,
				y * _q.w - w * _q.y - x * _q.z + z * _q.x,
				z * _q.w - w * _q.z - y * _q.x + x * _q.y,
				w * _q.w + x * _q.x + y * _q.y + z * _q.z);

			return (*this);
		}

		/**
		 * \fn friend Quaternion operator * ( const Quaternion & _q, float _f );
		 * \param _q quaternion to multiply
		 * \param _f factor size
		 * \return _q scaled by _f
		 * \brief operator *
		 */
		Quaternion operator * (const Quaternion & _q, float _f)
		{
			return Quaternion(_q.x*_f, _q.y*_f, _q.z*_f, _q.w*_f);
		}

		/**
		 * \fn friend Quaternion operator / ( const Quaternion & _q, float _f );
		 * \param _q quaternion to divide
		 * \param _f factor size
		 * \return _q divided by _f
		 * \brief operator /
		 */
		Quaternion operator / (const Quaternion & _q, float _f)
		{
			//LOG_ASSERT((_f != 0.0f), "Divide by 0");
			return Quaternion(_q.x / _f, _q.y / _f, _q.z / _f, _q.w / _f);
		}

		/**
		 * \fn friend Quaternion operator + ( const Quaternion & _q1, const Quaternion & _q2 );
		 * \param _q1 quaternion
		 * \param _q2 quaternion
		 * \return _q1 added to q2
		 * \brief operator +
		 */
		Quaternion operator + (const Quaternion & _q1, const Quaternion & _q2)
		{
			return Quaternion(_q1.x + _q2.x, _q1.y + _q2.y, _q1.z + _q2.z, _q1.w + _q2.w);
		}

		/**
		 * \fn friend Quaternion operator - ( const Quaternion & _q1, const Quaternion & _q2 );
		 * \param _q1 quaternion
		 * \param _q2 quaternion
		 * \return _q1 subtracted to q2
		 * \brief operator -
		 */
		Quaternion operator - (const Quaternion & _q1, const Quaternion & _q2)
		{
			return Quaternion(_q1.x - _q2.x, _q1.y - _q2.y, _q1.z - _q2.z, _q1.w - _q2.w);
		}

		/**
		 * \fn friend Quaternion operator * ( const Quaternion & _q1, const Quaternion & _q2 );
		 * \param _q1 quaternion
		 * \param _q2 quaternion
		 * \return _q1 * _q2
		 * \brief operator *
		 */
		Quaternion operator * (const Quaternion & _q1, const Quaternion & _q2)
		{
			return Quaternion(_q1.x * _q2.w + _q1.w * _q2.x + _q1.z * _q2.y - _q1.y * _q2.z,
				_q1.y * _q2.w + _q1.w * _q2.y + _q1.x * _q2.z - _q1.z * _q2.x,
				_q1.z * _q2.w + _q1.w * _q2.z + _q1.y * _q2.x - _q1.x * _q2.y,
				_q1.w * _q2.w - _q1.x * _q2.x - _q1.y * _q2.y - _q1.z * _q2.z);
		}

		/**
		 * \fn friend Quaternion operator / ( const Quaternion & _q1, const Quaternion & _q2 );
		 * \param _q1 quaternion
		 * \param _q2 quaternion
		 * \return _q1 / _q2
		 * \brief operator /
		 */
		Quaternion operator / (const Quaternion & _q1, const Quaternion & _q2)
		{
			return Quaternion(_q1.x * _q2.w - _q1.w * _q2.x - _q1.z * _q2.y + _q1.y * _q2.z,
				_q1.y * _q2.w - _q1.w * _q2.y - _q1.x * _q2.z + _q1.z * _q2.x,
				_q1.z * _q2.w - _q1.w * _q2.z - _q1.y * _q2.x + _q1.x * _q2.y,
				_q1.w * _q2.w + _q1.x * _q2.x + _q1.y * _q2.y + _q1.z * _q2.z);
		}
	}
}